#include "stm32f4xx.h"
#include "Delay.h"
#include "bsp_led.h"
#include <string.h>
#include "bsp_tim.h"
#include "bsp_adc.h"
#include "bsp_usart.h"
#include "bsp_spi.h"
#include "bsp_i2c.h"
#include "bsp_mpu6050.h"
#include "imu_quat.h"
#include "filter.h"
#include "bsp_motor.h"
#include "motorencoder.h" 
#include "pid.h"
#include "bsp_track.h"
#include "bsp_key.h"
#include "control.h"
#include "bsp_oled.h"

/* 变量 */
MPU6050_t* MPU6050 = NULL;
EulerAngle_t angle = {0, 0, 0};
float Yaw = 0;
float currentA = 0, currentB = 0;
PID_t pidMotorA;
PID_t pidMotorB;
PID_t pidAngle;

/* 标志位 */
uint8_t carMode = 0;        // 小车运动模式
uint8_t carRun = 1;         // 小车启动标志位
/* pid参数 */
float pidParam_MotorA[3] = {5, 0.3, 0.00};// pid的初始参数，可通过上位机来调试
float pidParam_MotorB[3] = {5, 0.3, 0.00};
float pidParam_Angle[3] = {1, 0.02, 0.0};

int main()
{
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    LED_Init();
    KEY_Init();
    USART_Config(115200);
    // printf("Hello World!\n");
    TB6612_Init(168-1, 100-1);// 电机频率10KHz
    I2C_Config();
    Track_Init();
    Encoder_Init();
    PID_Init(&pidMotorA, DELTA_PID, pidParam_MotorA[0], pidParam_MotorA[1], pidParam_MotorA[2]);
    PID_Init(&pidMotorB, DELTA_PID, pidParam_MotorB[0], pidParam_MotorB[1], pidParam_MotorB[2]);
    PID_Init(&pidAngle, POSITION_PID, pidParam_Angle[0], pidParam_Angle[1], pidParam_Angle[2]);
    Motor_Control(Motor_A, 1, 0);   // 停车
    Motor_Control(Motor_B, 1, 0);
    MPU6050 = MPU6050_Init();
    /* OLED 显示 */
    OLED_Init();
    OLED_ShowString(0, 16*3, "Y", OLED_8X16);
    OLED_ShowString(0, 0, "A", OLED_8X16);// 显示电机A的pid
    OLED_ShowFloatNum(8*(1), 0, pidParam_MotorA[0], 1, 2, OLED_8X16);
    OLED_ShowFloatNum(8*(1+5), 0, pidParam_MotorA[1], 1, 2, OLED_8X16);
    OLED_ShowFloatNum(8*(1+10), 0, pidParam_MotorA[2], 1, 2, OLED_8X16);
    OLED_ShowString(0, 16, "B", OLED_8X16);// 显示电机B的pid
    OLED_ShowFloatNum(8*(1), 16, pidParam_MotorB[0], 1, 2, OLED_8X16);
    OLED_ShowFloatNum(8*(1+5), 16, pidParam_MotorB[1], 1, 2, OLED_8X16);
    OLED_ShowFloatNum(8*(1+10), 16, pidParam_MotorB[2], 1, 2, OLED_8X16);
    OLED_ShowString(0, 16*2, "O", OLED_8X16);// 显示角度环的pid
    OLED_ShowFloatNum(8*(1), 16*2, pidParam_Angle[0], 1, 2, OLED_8X16);
    OLED_ShowFloatNum(8*(1+5), 16*2, pidParam_Angle[1], 1, 2, OLED_8X16);
    OLED_ShowFloatNum(8*(1+10), 16*2, pidParam_Angle[2], 1, 2, OLED_8X16);
    OLED_Update();
    Timer_Init(168-1, 3000-1);// 定时3ms
    while (1)
    {
        switch (carMode)
        {
        case 0:
            /* 上位机PID调参 */
            Serial_UpdateParam();
            OLED_ShowFloatNum(8*(1), 16*3, currentA, 4, 1, OLED_8X16);
            OLED_ShowFloatNum(8*(1+7), 16*3, currentB, 4, 1, OLED_8X16);
            OLED_Update();

            // LED_G_TOGGLE;
            // Delay_ms(1000);
            break;
        case 1: /* 第一问 */
            if (carRun == 1) {
                // while (MPU6050->devID != 0x68) {
                //     MPU6050 = MPU6050_Init();
                //     Timer_Init(168-1, 3000-1);// 定时3ms
                // }
                Control_AB();
            }
            break;
        case 2: /* 第二问 */
            if (carRun == 1) {
                Control_ABCDA();
            }
            break;
        case 3: /* 第三问 */
            if (carRun == 1) {
                Control_ACBDA();
            }
            break;
        case 4: /* 第四问 */
            if (carRun == 1) {
                Control_ACBDAx4();
            }
            break;
        default:
            break;
        }
    }
}
